#include "../include/calibrator.h"

bool readImages(std::string& path,const int& size,std::vector<cv::Mat> &mats)
{
    for(int i = 1;i<=size;++i)
    {
        cv::Mat tmp = cv::imread(path+std::to_string(i)+".jpg");
        if(tmp.empty())
        {
            std::cout<<"图片打开失败"<<std::endl;
            return false;
        }
        mats.push_back(tmp);
    }
    return true;
}

void calibration()
{
    fytVision::Calibrator calibrator;
    std::string path = "/home/";
    int size = 0;
    std::vector<cv::Mat> mats;
    readImages(path,size,mats);
    
    cv::Size img_size{800,600};
    cv::Size2f block_size{26.2,26.2};
    cv::Size board_size{6,10};

    calibrator.Init(mats,img_size,board_size,block_size);

    std::vector<cv::Mat> result;
    if(calibrator.Run())
    {
        calibrator.GetMats(result);
    }

    std::cout<<"cameraMatrix:  "<<std::endl<<result[0]<<std::endl<<std::endl;
    std::cout<<"distCoeffs:  "<<std::endl<<result[1]<<std::endl<<std::endl;
}

void takeImages()
{
    FightingCapture *capture = new FightingDahengCapture();
    cv::Mat src;
    std::vector<int> comprehenssion_pramas;
    comprehenssion_pramas.push_back(cv::IMWRITE_JPEG_QUALITY);
    comprehenssion_pramas.push_back(100);

    if(capture && !capture->init())
    {
        printf("Video source initialization failed.\n");
        delete capture;
    }
    printf("camera init success\n");
    int cnt = 0;
    int img_cnt = 0;
    while (true)
    {
        if(capture->read(src))
        {
            if(!src.empty())
            {
                if(cnt % 10 == 0)
                {

				    cv::imwrite("../images/"+std::to_string(cnt)+".jpg",src,comprehenssion_pramas);
                    img_cnt++;
                }
                cv::imshow("src",src);
                cv::waitKey(30);
            }
        }
        cnt++;
        if(img_cnt > 35)
        {
            break;
        }
    }
    
}

int main(int argc,char* argv[])
{
    if(argc > 1 && strcmp(argv[1],"camera") == 0)
    {
        takeImages();
    }
    else
    {
        calibration();        
    }
    return 0;
}